# Blob Detection Example
#
# This example shows off how to use the find_blobs function to find color
# blobs in the image. This example in particular looks for dark green objects.

import sensor, image, time
import car
from pid import PID

# You may need to tweak the above settings for tracking green things...
# Select an area in the Framebuffer to copy the color settings.

sensor.reset() # 初始化感光元件
sensor.set_pixformat(sensor.RGB565) # 设置颜色格式为RGB565，彩色，每个像素16bit
sensor.set_framesize(sensor.QQVGA) # 图像大小为 QQVGA 
sensor.skip_frames(10)          # 跳过n张照片，在更改设置后，跳过一些帧，等待感光元件变稳定。
sensor.set_auto_whitebal(False) # 颜色识别必须关闭白平衡，会影响颜色识别效果，导致颜色的阈值发生改变
clock = time.clock() # 跟踪FPS。

# 要想很好地跟踪颜色，理想情况下你应该非常，非常，非常受控的环境，照明是恒定的...
#小球的颜色
green_threshold   = (76, 96, -110, -30, 8, 66)
#根据像素大小识别小车与小球的距离，如果小于2000就追踪小球，如果大于2000就远离小球
size_threshold = 2000
#方向PID，控制电机方向，如果再小车拐角处，拐角过大，P设置小一点
x_pid = PID(p=0.5, i=1, imax=100)
#距离PID,控制小车的速度。如果小车速度过快，调节P小一点
h_pid = PID(p=0.05, i=0.1, imax=50)

#寻找最大的小球
def find_max(blobs):
    max_size=0
    for blob in blobs:
        #因为blobs是一个列表，这个列表里面包含了很多个元组，元组的第三个和第四个元素是边界框的宽度与高度
        if blob[2]*blob[3] > max_size:
            max_blob=blob
            max_size = blob[2]*blob[3]
    return max_blob

while(True):
    clock.tick() # 追踪两个snapshots()之间经过的毫秒数
    img = sensor.snapshot() # 截取感光元件中的一张图片
    
    blobs = img.find_blobs([green_threshold]) #如果识别到绿色才会返回参数，参数为一个包括每个色块的色块对象的列表，否则blobs为0
    if blobs:
        max_blob = find_max(blobs)  #获取最大的绿色区域，因为小球的一定是识别区域占地最大的地方
        x_error = max_blob[5]-img.width()/2  #用于方向PID的，计算色块的中心x位置与摄像头x中心位置距离
        h_error = max_blob[2]*max_blob[3]-size_threshold #用于距离PID的，计算识别到的小球像素点大小与规定的像素点大小的差值
        print("x error: ", x_error)
        '''
        for b in blobs:
            # Draw a rect around the blob.
            img.draw_rectangle(b[0:4]) # rect
            img.draw_cross(b[5], b[6]) # cx, cy
        '''
        img.draw_rectangle(max_blob[0:4]) # 找到的色块上面画一个矩形
        img.draw_cross(max_blob[5], max_blob[6]) # 找到的色块上面画一个十字
        x_output=x_pid.get_pid(x_error,1)  #调用方向POD
        h_output=h_pid.get_pid(h_error,1)  #调用距离PID
        print("h_output",h_output)
        car.run(-h_output-x_output,-h_output+x_output) #设置左右轮转速
    else:    #如果没有找到色块，就让小车缓慢的旋转
        car.run(18,-18)
